#include "grap.h"

 int set_grapspd[8];
 int set_angle[10] = {0};
 int djangle = 800;
 int PercentG=0;
 pid_t pid_grapspd[8],pid_grappos[8];
 moto_measure_t moto_grapchassis[8];
 
 //id为电调编号
void init_grap(CAN_HandleTypeDef* hcangrap,int id1,int id2,int id3,int id4)
{
//	for(int i=0;i<4;i++){
//	moto_grapchassis[i] = moto_chassis[i];
//	}
	 // 初始化滤波器
  TIM5->CCR2 = djangle;
	TIM5->CCR1 = djangle;
	my_can_filter_init_recv_all(hcangrap);
	//HAL_CAN_Receive_IT(hcangrap, CAN_FIFO0);
	for(int i=0;i < 8;i++){
		PID_struct_init(&pid_grapspd[i], POSITION_PID, 20000.0f, 20000,
									0.5f,	0.1f,	0.15f	);  //0.5 0.1 0.1
	}
//	PID_struct_init(&pid_grappos[id1], POSITION_PID, 2000.0f, 500,
//									0.05f,	-0.1f,	8.4f	);  //4 motos angular rate closeloop.
	PID_struct_init(&pid_grappos[id1], POSITION_PID, 2000.0f, 50,
									0.1f,	0.25f,	8.9f	); //2500 50 0.1 0.15 8.9
	PID_struct_init(&pid_grappos[id1], POSITION_PID, 2000.0f, 50,
									0.1f,	0.25f,	8.9f	); //2500 50 0.1 0.15 8.9
	
}
void grap(CAN_HandleTypeDef* hcangrap,int id1,int id2,int id3,int id4)
{
	//moto_chassis[id1].total_angle = moving_average_filter(&filter, moto_chassis[id1].total_angle);    // 应用滤波器
	//my_can_filter_init_recv_all(&hcangrap);
	for(int i=0;i<8;i++){
	pid_calc(&pid_grappos[i], moto_chassis[i].total_angle, set_angle[i]);
	}
			set_grapspd[id1] = pid_grappos[id1].pos_out;
			//printf("%d\r\n",set_grapspd[id1]);
//           
//			set_grapspd[id2] = pid_grappos[id2].pos_out;
//	//printf("g=%d\r\n",moto_chassis[2].total_angle);
			for(int i=0; i<8; i++)
			{
				pid_calc(&pid_grapspd[i], moto_chassis[i].speed_rpm, set_grapspd[i]);
			}
			
			set_moto_current(0x1FF,hcangrap, pid_grapspd[id1].pos_out, 
									pid_grapspd[id2].pos_out,
									pid_grapspd[id3].pos_out,
									pid_grapspd[id4].pos_out);
}


//用于调试的
void graprev(char *pRx_Data,int id1)
{
	if(pRx_Data[0]=='g')
		{
			djangle = 1000*(pRx_Data[1]- '0');
			djangle += 100*(pRx_Data[2]- '0');
			djangle += 10*(pRx_Data[3]- '0');
			TIM5->CCR2 = djangle;
			
		}
		else if(pRx_Data[0]=='1') 
		{
			set_angle[id1] = -100000*(pRx_Data[1]- '0');
			set_angle[id1] += -10000*(pRx_Data[2]- '0');
			set_angle[id1] += -1000*(pRx_Data[3]- '0');
			set_angle[id1] += -100*(pRx_Data[4]- '0');
			
		}
		else if(pRx_Data[0]=='2') 
		{
			set_angle[2] = 100000*(pRx_Data[1]- '0');
			set_angle[2] += 10000*(pRx_Data[2]- '0');
			set_angle[2] += 1000*(pRx_Data[3]- '0');
			set_angle[2] += 100*(pRx_Data[4]- '0');
		}
}

void Autograp(char *pRx_Data,int id1)
{
	if(pRx_Data[0]=='d')
	{	
		set_angle[id1] = -16000;
		grap(&hcan1,1,0,0,0);
		set_angle[id1] = -38000;
	}
	else if(pRx_Data[0]=='u')
	{
		set_angle[id1] = 0;
	}
	else if(pRx_Data[0]=='p')
	{
		TIM5->CCR2 = 1840;
	}
	else if(pRx_Data[0]=='g')
	{
		TIM5->CCR2 = 1700;
	}
}

